#include "pr.h"
#include "./TreeRouting.h"
#include "printf.h"

generic module CtpRoutingEngineP(uint8_t routingTableSize, uint32_t minInterval, uint32_t maxInterval){
	provides interface UnicastNameFreeRouting as Routing;
	provides interface RootControl;
	provides interface StdControl;
	provides interface Init;
	provides interface CtpInfo;
	//omit provides interface CtpRoutingPacket;

	uses interface Timer<TMilli> as BeaconTimer;
	uses interface Timer<TMilli> as CheckNeighborTabelTimer;
	uses interface AMSend as BeaconSend;
	uses interface Receive as BeaconReceive;
	uses interface LinkEstimator;
	//use interface CompareBit;
	uses interface Random;
	uses interface SplitControl as RadioControl;
	uses interface CollectionDebug;
	uses interface MyLeds as Leds;

}

implementation{
	
	
	bool sending = FALSE;
	beacon_info_t* beaconMsg;
	uint8_t BeaconHop;
	uint8_t DefaultHop;
	uint8_t needBeacon;
	message_t beaconMsgBuffer;
	routing_table_entry routingTable[routingTableSize];
	uint8_t routingTableActive;
	parent_info_t parentInfo;
	bool setfwder = FALSE;
	//bool startBeacon = FALSE;
	uint32_t currentInterval = minInterval;
	uint32_t tempInterval;
	bool runing = FALSE;
	bool radioOn = FALSE;
	bool state_is_root;
	bool hasNeighbor = FALSE;
	uint16_t beaconCount;
	uint8_t needBeacon_temp;
	
	void chooseAdvertiseTime() {
       //if(startBeacon || state_is_root) {
			tempInterval = currentInterval;
			tempInterval /= 2;
			tempInterval += call Random.rand32() % tempInterval;
			//tHasPassed = FALSE;
			call BeaconTimer.stop();
			call BeaconTimer.startOneShot(tempInterval);
	  // }
    }
	
	void decayInterval() {
		currentInterval *= 2;
		if(currentInterval > maxInterval) {
			currentInterval = maxInterval;
		}
		chooseAdvertiseTime();
	}	
	void resetInterval(){
		currentInterval = minInterval;
		chooseAdvertiseTime();
	}
	
	void routingTableInit();
	void checkTimeoutNeighbor(uint32_t ts);
	uint8_t checkTabelMinHop();
	error_t routingTableUpdateEntry(am_addr_t from, uint8_t rssi, uint8_t hop,uint32_t rcvTime);
	
	command error_t Init.init() {
		radioOn = FALSE;
		runing = FALSE;
		state_is_root = FALSE;
		routingTableInit();
		needBeacon = 0;
		beaconMsg = call BeaconSend.getPayload(&beaconMsgBuffer, call BeaconSend.maxPayloadLength());
		DefaultHop = 0xff;
		chooseAdvertiseTime();	
		call CheckNeighborTabelTimer.startOneShot(CHECKTIMEOUTNEIGHBORTIMER);
		parentInfo.parent = INVALID_ADDR;
		parentInfo.rssi = 0;
		parentInfo.hop = INVALID_HOP;
		return SUCCESS;
	}
	
	event void CheckNeighborTabelTimer.fired() {
		uint32_t ts = call CheckNeighborTabelTimer.getNow();
		checkTimeoutNeighbor(ts);
		call CheckNeighborTabelTimer.startOneShot(CHECKTIMEOUTNEIGHBORTIMER);
	}
	
	
	void checkTimeoutNeighbor(uint32_t ts) {
		uint8_t i;
		uint8_t j;
		//In routingTableInit(), routingTableActive is 0.
		//For the nodes who have no neighbor, its routingTableActive is 0. 
		//Then they will not go into this for loop.
		for (i = 0; i < routingTableActive; i++) {
			if(ts - routingTable[i].rcvTime > TIMEOUTNEIGHBOR) {
				// if any neighbor down/timeout, then it needs beacon itself.
				needBeacon = 1;		// request  neighbor beacon
				// if the timeout neighbor is the last one, omit it.
				if(i == routingTableActive -1) {
					routingTableActive -= 1;
				}
				//otherwise, reorder the neighbor table, make no space in the table.
				else {
					for(j = i;j < routingTableActive - 1; j++) {
						routingTable[j] = routingTable[j + 1];
					}
					routingTableActive -= 1;
				}
				//here are some problems. 
				//1.What you want is to check whether the timeout neighbor is its parent.
				//However, you have updated the neighbor table, the parent changing failed
				//since you have already deleted the timeout neighbor.
				//1.Solution: put this checking before updating neighbor table.
				//2.Besides, if the timeout neighbor is the parent, where do you update the parent info?
				//It seems you just make its parent disabled, could you update the parent info here?
				//I beleive this will reduce the time of no parent or wrong parent state.
				//2.Solution: To Be Determined(TBD).
				if(routingTable[i].neighbor == parentInfo.parent) {
					parentInfo.rssi = 0;
				}
			}
		}
	}
	
	
	command error_t StdControl.start() {
		if(!runing) {
			runing = TRUE;
			resetInterval();
		}
		return SUCCESS;
	}
	command error_t StdControl.stop() {
		runing = FALSE;
		return SUCCESS;
	}
	
	event void RadioControl.startDone(error_t error) { 
		radioOn = TRUE;
	}
	
	
	event void RadioControl.stopDone(error_t error) {
		radioOn = FALSE;
	}
	
	
	event message_t* BeaconReceive.receive(message_t* msg, void* payload, uint8_t len){
		beacon_info_t* rcvBeacon;
		am_addr_t from;
		uint8_t rssi;
		uint8_t hop;
		uint32_t rcvTime;
		//pr("Rssi_0\n");
		call Leds.led1Toggle();
		if(len != sizeof(beacon_info_t)) {
			return msg;
		}
		rcvBeacon = (beacon_info_t*)payload;
		rcvTime = call CheckNeighborTabelTimer.getNow();
		from = rcvBeacon -> nodeid;
		rssi = call LinkEstimator.getRssi(msg);
		hop = rcvBeacon -> hop;
		
		needBeacon_temp = rcvBeacon -> needBeacon;
		if(needBeacon_temp == 1) {
			resetInterval();
		}
		
		pr("rcv Pkt Rssi = %d ,nodeid = %d , hop = %d \n",rssi,from,hop);
		if(rcvBeacon -> hop != INVALID_HOP) {
			//pr("Rssi_1\n");
			if((rssi > RCVPKT_RSSI_THRESHOLD )|| (rssi < 127)){
				pr("Rssi OK : nodeid = %d , hop = %d\n", rcvBeacon -> nodeid,rcvBeacon -> hop);
				if (rcvBeacon -> nodeid != INVALID_ADDR) {
					routingTableUpdateEntry(from,rssi,hop,rcvTime);
				}
			}
		}
		return msg;
	}

	task void sendBeaconTask(){
		pr("BeaconSend\n");
		if(sending) {
			return;
		}
		pr("hasNeighbor = %d state_is_root = %d\n",hasNeighbor,state_is_root);
		//if it has a neighbor and it is not root, then broadcast its own hop count.
		if(hasNeighbor && !state_is_root) {
			call Routing.nextHop();
			//define its own hop count to sink 
			DefaultHop = parentInfo.hop + 1;
			//broadcast its hop count to others.
			beaconMsg -> hop = DefaultHop;
			pr("DefaultHop1 = %d\n",DefaultHop);
		}
		//If it is root, hop count is 0.
		else if(state_is_root){
			beaconMsg -> hop = 0;      // root
		}
		//If it has no neighbor and it is not root, it use default hop in Bcast message.
		//Default hop count is 0xff, to annouce it has no neighbor.
		else {
			beaconMsg -> hop = DefaultHop; 
			pr("DefaultHop2 = %d\n",DefaultHop);
		}
		beaconMsg -> nodeid = TOS_NODE_ID;
		beaconMsg -> needBeacon = needBeacon;
		call BeaconSend.send(AM_BROADCAST_ADDR,&beaconMsgBuffer,sizeof(beacon_info_t));
		beaconCount++;
		sending = TRUE;
		//needBeacon means? need others' beacon to find neighbor? YES
		if(needBeacon == 1) {
			needBeacon = 0;
		}
	}
	
	event void BeaconTimer.fired(){
		if(runing && radioOn) {
			post sendBeaconTask();
			//call Leds.led0Toggle();
		}
	}
	
	event void BeaconSend.sendDone(message_t* msg, error_t error){
		if(error == SUCCESS) {
			sending = FALSE;
			decayInterval();
		}
	}
	
	
	command error_t RootControl.setRoot() {
		state_is_root = TRUE;
		DefaultHop = 0;
		//chooseAdvertiseTime();	
		return SUCCESS;
	}
	
	command bool RootControl.isRoot() {
		return state_is_root;
	}
	
	
	
	//event bool CompareBit.shouldInsert(message_t *msg, void* payload, uint8_t len) {
	//}
	
	void routingTableInit() {
        routingTableActive = 0;
    }
	
    uint8_t routingTableFind(am_addr_t neighbor) {
        uint8_t i;
        if (neighbor == INVALID_ADDR)
            return routingTableActive;
        for (i = 0; i < routingTableActive; i++) {
            if (routingTable[i].neighbor == neighbor)
                break;
        }
        return i;
    }
	
	uint8_t routingTableCheck(am_addr_t from, uint8_t rssi, uint8_t hop) {
		uint8_t i;
		//uint8_t Diff_Rssi;
		//uint8_t idx;
		//uint8_t Less_Threshold;
		for (i = 0; i < routingTableSize; i++) {
		           if ((rssi - routingTable[i].rssi) > NEIGHBORREPLACE_RSSI 
						&& routingTable[i].neighbor != parentInfo.parent) {
		                		//Diff_Rssi = rssi - routingTable[i].rssi;
					//idx = i;
					return i;
					//you have return i here,why you need break?
					break;
			}
			//I think the place where you want to do  "break;" is here!
			//Besides, I think here you should use "continue;", because you want to check all the neighbors in the table.
			//Since you only check the first neighbor, whose id==0, then 
			//if the first neighbor is not meeted the condition, then it will always return routingTableSize. 
			return routingTableSize;  //  i + 1 = routingTableSize
		}
		/*if(Less_Threshold < routingTableSize){
			return i;
		}
		else {
			return Less_Threshold;
		}*/
	}
	
	uint8_t checkTabelMinHop() {
		uint8_t temp_hop = 0xff;
		uint8_t i;
		for (i = 0; i < routingTableActive; i++) {
			if(routingTable[i].hop < temp_hop) {
				temp_hop = routingTable[i].hop;
			}
		}
		return temp_hop;
	}
	
	error_t routingTableUpdateEntry(am_addr_t from, uint8_t rssi, uint8_t hop, uint32_t rcvTime) {
		uint8_t idx;
		uint8_t k;
		idx = routingTableFind(from);
		pr("idx = %d\n",idx);
		//call Leds.led0Toggle();
		if(idx == routingTableSize) {
			//Here is a problem. You may return routingTableSize all the time, then you cannot find the worst neighbor. 
			//Detial in the implementation of routingTableCheck();
			k = routingTableCheck(from,rssi,hop);  // check which is worst
			if(k < routingTableSize) {
				atomic {
					routingTable[k].neighbor = from;
					routingTable[k].rssi = rssi;
					routingTable[k].hop = hop;
					routingTable[k].rcvTime = rcvTime;
				}
			}
		}
		else if(idx == routingTableActive) {   // Tabel not full
			call Leds.led0On();
			atomic {
				routingTable[idx].neighbor = from;
				routingTable[idx].rssi = rssi;
				routingTable[idx].hop = hop;
				routingTable[idx].rcvTime = rcvTime;
				routingTableActive++;
			}
		}
		else {
			//call Leds.led1Toggle();
			atomic {                           // update neighbor info
				routingTable[idx].neighbor = from;
				routingTable[idx].rssi = rssi;
				routingTable[idx].hop = hop;
				routingTable[idx].rcvTime = rcvTime;
			}
			if(from == parentInfo.parent) {
				parentInfo.rssi = rssi;
				parentInfo.hop = hop;
			}
		}
		hasNeighbor = TRUE;
		pr("routingTableActive = %d\n",routingTableActive);
		for(k = 0;k < routingTableActive;k++) {
			pr("from = %d rssi = %d hop = %d\n",routingTable[k].neighbor,routingTable[k].rssi,routingTable[k].hop);
		}
		pr("has update\n");
		return SUCCESS;	
	}
	
	error_t parentChange() {
		uint8_t i;
		uint8_t j;
		i = routingTableFind(parentInfo.parent);
		if(i == routingTableSize) {
			//
		}
		else {
			//Here I don't understand. Why you delete this parent? 
			//Since the parent changes may because the rssi is become worse, but it is still a neighbor. Why delete it?
			//Please check your logic here once again.
			for(j = i;j < routingTableActive - 1; j++) {
				routingTable[j] = routingTable[j + 1];
			}
			//here is a problem. Consider a node has no parent, then parentInfo.parent==INVALID_ADDR,
			//then i is routingTableActive(which should be 0), based on your implementation of routingTableFind();
			//then skip above for loop and reduce routingTableActive by 1.
			// routingTableActive will become -1.???Please check it again.
			routingTableActive -= 1;
		}
		setfwder = FALSE;
		call Routing.nextHop();
		return SUCCESS;
	}
	
	command am_addr_t Routing.nextHop() {
		uint8_t i;
		uint8_t temp_hop;
		if(setfwder) {
			temp_hop = checkTabelMinHop();
			for (i = 0; i < routingTableActive; i++) {
				if (routingTable[i].hop == temp_hop) {
					if(temp_hop < DefaultHop - 1) {  // hop less than DefaultHop -1 
						parentInfo.rssi = routingTable[i].rssi;
						parentInfo.parent = routingTable[i].neighbor;
						parentInfo.hop = routingTable[i].hop;
						break;
					}
					else if(temp_hop == DefaultHop - 1){  //hop equal DefaultHop - 1
						if(routingTable[i].rssi - parentInfo.rssi > PARENT_SWITCH_THRESHOLD) {
							parentInfo.rssi = routingTable[i].rssi;
							parentInfo.parent = routingTable[i].neighbor;
							parentInfo.hop = routingTable[i].hop;
						}
					}
					DefaultHop = parentInfo.hop + 1;
				}
			}
			return parentInfo.parent;
		}
		else {
			temp_hop = checkTabelMinHop();
			for (i = 0; i < routingTableActive; i++) { 
				if(routingTableActive == 1) {
					parentInfo.rssi = routingTable[i].rssi;
					parentInfo.parent = routingTable[i].neighbor;
					parentInfo.hop = routingTable[i].hop;
				}
				else if(routingTable[i].hop == temp_hop) {
					if(routingTable[i].rssi > parentInfo.rssi) {
						parentInfo.rssi = routingTable[i].rssi;
						parentInfo.parent = routingTable[i].neighbor;
						parentInfo.hop = routingTable[i].hop;
					}
				}
				setfwder = TRUE;
				DefaultHop = parentInfo.hop + 1;
			}
			return parentInfo.parent;
		}
	}
	
	command bool Routing.hasRoute() {
		parentInfo.parent = call Routing.nextHop();
		return(parentInfo.parent != INVALID_ADDR);
	}
	
	command void CtpInfo.needBeacon() {
		needBeacon = 1;
	}
	
	command void CtpInfo.parentNeedChange() {
		parentChange();
	}
	
	command uint16_t CtpInfo.getBeaconCount() {
		return beaconCount;
	}
	
	command bool CtpInfo.getParent(am_addr_t* parent){
		if(parent == NULL)
			return FAIL;
		if(parentInfo.parent == INVALID_ADDR)
			return FAIL;
		*parent = parentInfo.parent;
		return SUCCESS;
	}
	
	command uint8_t CtpInfo.numNeighbors() {
      return routingTableActive;
    }
	
	command am_addr_t CtpInfo.getNeighborAddr(uint8_t n) {
		return (n < routingTableActive)? routingTable[n].neighbor:AM_BROADCAST_ADDR;
	}
	
	command uint8_t CtpInfo.getRssi(uint8_t n) {
		return (n < routingTableActive)? routingTable[n].rssi:0xff;
	}
	
	
	
	default event void Routing.noRoute() {
    }
    
    default event void Routing.routeFound() {
    }

	
	
	
}